#pragma once
#include<iostream>
#include<opencv2/opencv.hpp>
#include"Lightbar.hpp"
#include"Draw_Contours.hpp"
#include<list>
#include<algorithm>
// #include<fmt/format.h>
#include"ArmorPose.hpp"



class Armor{

public:

    //     长度     短比例     长比例     离装甲板的距离   两等条比例  两灯条角度误差  两灯条中点向量与x轴正方向的夹角
    double length,ratio_min,ratio_max,armorDistance,side_ratio,errorAngleBar,armorAngle;
    Lightbar left,right; // 左右灯条
    vector<Point>corners; // 装甲板角点容器
    Point2f firePoint;
    ArmorPatten::Name robotName; 
    Mat roiImg;// ROI
    Mat rvec,tvec; // 位姿

    Armor():
    ratio_max(0),
    ratio_min(0),
    length(0),
    armorDistance(0),
    firePoint(0,0)
    {};

    // // 拷贝构造
    // Armor(const Armor& other){ 
    //     this->roiImg = other.roiImg.clone(); 
    //     this->rvec = other.rvec.clone(); 
    //     this->tvec = other.tvec.clone(); 
    //     this->robotName = other.robotName; 
    //     this->length = other.length;
    //     this->ratio_min = other.ratio_min;
    //     this->ratio_max = other.ratio_max;
    //     this->armorDistance = other.armorDistance;
    //     this->side_ratio = other.side_ratio;
    // };

    Armor(const Lightbar &left,const Lightbar &right,Mat &img){

        this->corners.push_back(Point(saturate_cast<int>(left.topPoint.x),saturate_cast<int>(left.topPoint.y)));
        this->corners.push_back(Point(saturate_cast<int>(right.topPoint.x),saturate_cast<int>(right.topPoint.y)));
        this->corners.push_back(Point(saturate_cast<int>(right.bottomPoint.x),saturate_cast<int>(right.bottomPoint.y)));
        this->corners.push_back(Point(saturate_cast<int>(left.bottomPoint.x),saturate_cast<int>(left.bottomPoint.y)));
        this->left = left;
        this->right = right;
        this->length  = norm(left.midPoint - right.midPoint);
        auto max_lightbar_length = std::max(left.height, right.height);
        auto min_lightbar_length = std::min(left.height, right.height);
        this->ratio_min = length / min_lightbar_length;                     //装甲板的长宽比例
        this->ratio_max = length / max_lightbar_length;                     //装甲板的长宽比例
        this->side_ratio = max_lightbar_length / min_lightbar_length;  //旋转角度产生的灯条大小问题
        // double width = (left.height + right.height)/2;
        // this->angleArmor = atan2(ArTop2ArBot.y,ArTop2ArBot.x);
        this->errorAngleBar = abs(left.angle - right .angle);
        Point2f leftM2rightM = right.midPoint - left.midPoint;
        this->armorAngle = abs(std::atan2(leftM2rightM.y, leftM2rightM.x));  //两灯条中点向量与x轴正方向的夹角

        // double roll = atan2(leftM2rgihtM.y, leftM2rgihtM.x);  //两灯条中点向量与x轴正方向的夹角
        // auto L_errorAngle = abs(left.angle - roll - CV_PI /  2);
        // auto R_errorAngle = abs(right.angle - roll - CV_PI /  2);
        // this->error_angleArmor = max(L_errorAngle,R_errorAngle); // 获取大的误差

        // 计算轮廓面积
        // this->armorArea = contourArea(corners); 
        // 周长
        // this->rect_c = arcLength(corners,true);
        
        //---------------------测试------------------------
        // line(img,topMidPoint,bottomMidPoint,Scalar(0,0,200),2);
        // line(img,left.midPoint,right.midPoint,Scalar(0,0,200),2);
        // putText(img,"m_len"+to_string(norm(left.midPoint-right.midPoint)),topMidPoint-Point2f(0,20),FONT_HERSHEY_SIMPLEX,0.7,Scalar(255,40,100),2);
        // putText(img,"m_width"+to_string(norm(topMidPoint-bottomMidPoint)),left.midPoint-Point2f(20,0),FONT_HERSHEY_SIMPLEX,0.7,Scalar(255,40,100),2);
        // 边
        // putText(img,"width: " + to_string(min_lightbar_length*2.45),left.midPoint,FONT_HERSHEY_SIMPLEX,0.7,Scalar(255,40,100),2);
        // putText(img,"l+w: " + to_string(this->length+width),Point(10,80),FONT_HERSHEY_SIMPLEX,0.7,Scalar(255,40,100),2);
        // putText(img,"rect_c: "+to_string(rect_c),Point(10,60),FONT_HERSHEY_COMPLEX,0.7,Scalar(150,200,240),2);
        // putText(img,"result: "+to_string(abs((this->length+width)*2 - this->rect_c)),Point(10,100),FONT_HERSHEY_COMPLEX,0.7,Scalar(150,25,240),2);

        // putText(img,"l_width: " + to_string(left.height),left.midPoint,FONT_HERSHEY_SIMPLEX,0.7,Scalar(255,40,100),2);
        // putText(img,"r_width: " + to_string(right.height),right.midPoint,FONT_HERSHEY_SIMPLEX,0.7,Scalar(255,40,100),2);
        //putText(img,"t_length: " + to_string(norm(left.topPoint - right.topPoint)),topMidPoint+Point2f(-5,-10),FONT_HERSHEY_SIMPLEX,0.7,Scalar(0,40,100),2);
        //putText(img,"b_length: " + to_string(norm(left.bottomPoint - right.bottomPoint)),bottomMidPoint+Point2f(-5,-10),FONT_HERSHEY_SIMPLEX,0.7,Scalar(0,40,100),2);
        // 角度
        // putText(img,"l_error_angle: "+ to_string(left.error_angle*57.3),left.bottomPoint+Point2f(0,30),FONT_HERSHEY_SIMPLEX,0.7,Scalar(0,40,100),2);
        // putText(img,"r_error_angle: "+ to_string(right.error_angle*57.3),right.bottomPoint+Point2f(0,30),FONT_HERSHEY_SIMPLEX,0.7,Scalar(0,40,100),2);
        // putText(img,"errorAngleBar: "+ to_string(errorAngleBar*57.3),(left.topPoint + right.topPoint ) / 2+Point2f(10,-40),FONT_HERSHEY_SIMPLEX,0.7,Scalar(200,0,255),2);
        // putText(img,"L_errorAngle: "+to_string(L_errorAngle*57.3),left.bottomPoint+Point2f(0,30),FONT_HERSHEY_SIMPLEX,0.7,Scalar(0,40,100),2);
        // putText(img,"R_errorAngle: "+to_string(R_errorAngle*57.3),right.bottomPoint+Point2f(0,30),FONT_HERSHEY_SIMPLEX,0.7,Scalar(0,40,100),2);
        //putText(img,"armorAngle: "+ to_string(armorAngle*57.3),Point2f(10,140),FONT_HERSHEY_SIMPLEX,0.7,Scalar(100,250,90),2);
        // putText(img,"left_armorAngle: "+ to_string(abs(this->armorAngle - left.error_angle)*57.3),left.bottomPoint+Point2f(10,140),FONT_HERSHEY_SIMPLEX,0.7,Scalar(89,200,40),2);
        // putText(img,"right_armorAngle: "+ to_string(abs(this->armorAngle - right.error_angle)*57.3),right.bottomPoint+Point2f(10,140),FONT_HERSHEY_SIMPLEX,0.7,Scalar(89,200,40),2);
        // 比例
        //putText(img,"side_ratio: "+to_string(side_ratio*100),Point2f(10,120),FONT_HERSHEY_SIMPLEX,0.7,Scalar(89,200,200),2); // 左右两灯条的长度比例
        // putText(img,"ratio_max: " + to_string(ratio_max),Point(10,100),FONT_HERSHEY_SIMPLEX,0.7,Scalar(150,180,130),2);
        //putText(img,"ratio_min: " + to_string(ratio_min),Point(10,80),FONT_HERSHEY_SIMPLEX,0.7,Scalar(150,180,130),2);

        // putText(img,"rect_c: "+to_string(rect_c),Point(10,160),FONT_HERSHEY_SIMPLEX,0.7,Scalar(200,180,50),2);
        // putText(img,"armorArea: "+to_string(armorArea),Point(10,200),FONT_HERSHEY_SIMPLEX,0.7,Scalar(150,60,230),2);
        
        //  点
        //circle(img,topMidPoint,4,Scalar(10,20,255),-1);
        //circle(img,bottomMidPoint,4,Scalar(10,20,255),-1);
        // putText(img,"topMid",topMidPoint+Point2f(-10,10),FONT_HERSHEY_SIMPLEX,0.7,Scalar(0,40,255),2);
        // putText(img,"botMid",bottomMidPoint,FONT_HERSHEY_SIMPLEX,0.7,Scalar(0,40,255),2);

    }// Armor_end

    // 装甲板筛选函数的调用
    // static Armor getSelect(const list<Lightbar> &lightbars,Mat &bgr_image);
    
    // 获取图案
    void get_pattern(const Mat &bgr_img,const Lightbar & left,const Lightbar & right);

    // 装甲板姿态
    void armorPose(Mat &img);

    // 机器人判断
    void Name_Dcide();

};




